/***************************************************************************
 创建者: 华磊
 开始时间: 2020.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "ecatmasterbase.h"
#include "robotcontrol.h"
#include "robotbase.h"
#include "ecatbusmanager.h"
#include "time.h"
#include <sched.h>
#include <fcntl.h>
#include <unistd.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <qsettings.h>
#include "messagelog.h"
#include "timetool.h"

#define O_RDWR		     02
static int latency_target_fd = -1;
static int32_t latency_target_value = 0;

extern RobotControl* robCtl;

inline int set_cpu(int i)
{
    cpu_set_t mask;
    CPU_ZERO(&mask);
    CPU_SET(i,&mask);
    printf("thread %u, i = %d\n", pthread_self(), i);
    if(-1 == pthread_setaffinity_np(pthread_self() ,sizeof(mask),&mask))
    {
        fprintf(stderr, "pthread_setaffinity_np erro\n");
        return -1;
    }
    return 0;
}

/* Latency trick
 * if the file /dev/cpu_dma_latency exists,
 * open it and write a zero into it. This will tell
 * the power management system not to transition to
 * a high cstate (in fact, the system acts like idle=poll)
 * When the fd to /dev/cpu_dma_latency is closed, the behavior
 * goes back to the system default.
 *
 * Documentation/power/pm_qos_interface.txt
 */
static void set_latency_target(void)
{
    struct stat s;
    int err;

    //    if (laptop) {
    //        warn("not setting cpu_dma_latency to save battery power\n");
    //        return;
    //    }

    int err_no = 0;
    err = stat("/dev/cpu_dma_latency", &s);
    if (err == -1) {
        printf(  "WARN: stat /dev/cpu_dma_latency failed");
        return;
    }

    err_no = 0;
    latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
    if (latency_target_fd == -1) {
        printf(  "WARN: open /dev/cpu_dma_latency");
        return;
    }

    err_no = 0;
    err = write(latency_target_fd, &latency_target_value, 4);
    if (err < 1) {
        printf(  "# printf setting cpu_dma_latency to %d!", latency_target_value);
        close(latency_target_fd);
        return;
    }
    printf("dma set to %dus\n", latency_target_value);
}



EcatMasterBase::EcatMasterBase(int pthread_priority_in, int pthread_schedule_in)
{
    setServoDataFlag=false;
    pthread_priority=pthread_priority_in;
    pthread_schedule=pthread_schedule_in;
    cycle_us=1000;
    cycle_ns=cycle_us*1000;
    isRun=true;
    canquit=false;
    isShutDown=false;
    isrunning=false;
    masteroldState=eMasterUnknown;
    masterCurrentState=eMasterUnknown;
    masterCommandState=eMasterUnknown;
    isInitialedOk=false;
    dcTimerShift=0;
//    log=new EcatLog;
//    log->init();
    isSlavesOp=false;

    isFristTimeInOp=0;
    opLoopCount=0.0;
    timerErrorCount=0;
    receiveErrorCount=0;
    sendErrorCount=0;
    customErrorCount=0;


    // EtherCAT distributed clock variables
    dc_start_time_ns = 0LL;
    dc_time_ns = 0;
    #if SYNC_MASTER_TO_REF
    dc_started = 0;
    dc_diff_ns = 0;
    prev_dc_diff_ns = 0;
    dc_diff_total_ns = 0LL;
    dc_delta_total_ns = 0LL;
    dc_filter_idx = 0;
    dc_adjust_ns=0;
    #endif
    system_time_base = 0LL;
    wakeup_time = 0LL;
    overruns = 0LL;

    ecatPeriodTimerMonitor_max=0;
    ecatPeriodTimerMonitor_min=100000000000;
    ecatReceiveMonitor_max=0;
    ecatReceiveMonitor_min=100000000000;
    ecatSendMonitor_max=0;
    ecatSendMonitor_min=100000000000;
    ecatCustomMonitor_max=0;
    ecatCustomMonitor_min=100000000000;


    initialTimerMeasurement();



}

EcatMasterBase::~EcatMasterBase()
{
    while(!canquit){
        isRun=false;
        osSleepUs(cycle_us);
    }
}

int EcatMasterBase::controllerShutDown()
{
    setMasterState( eMasterSleep );
    return 1;
}

bool EcatMasterBase::isSlavesOpStatus()
{
    return isSlavesOp;
}

bool EcatMasterBase::isInitialOkStatus()
{
    return isInitialedOk;
}

int EcatMasterBase::setDcTimerShift(int shiftIn)
{
    dcTimerShift=shiftIn;
    cycleTimespec.tv_nsec += dcTimerShift;
    while (cycleTimespec.tv_nsec >= NSEC_PER_SEC)
    {
        cycleTimespec.tv_nsec -= NSEC_PER_SEC;
        cycleTimespec.tv_sec++;
    }
    return 1;
}

int EcatMasterBase::resetMasterTimeMeasure()
{
    ecatPeriodTimerMonitor_max=0;
    ecatPeriodTimerMonitor_min=100000000000;
    ecatReceiveMonitor_max=0;
    ecatReceiveMonitor_min=100000000000;
    ecatSendMonitor_max=0;
    ecatSendMonitor_min=100000000000;
    ecatCustomMonitor_max=0;
    ecatCustomMonitor_min=100000000000;
    return 1;
}

int EcatMasterBase::enableSetServoData(bool enable)
{
    setServoDataFlag=enable;
    return 1;
}

int EcatMasterBase::resetMasterStatus()
{
    masterCurrentState=eMasterInit;
    masterCommandState=eMasterOP;

    return 1;
}

int EcatMasterBase::setServoData(RobotBase* rob)
{
    if(true==setServoDataFlag)
    {
       rob->setServoData();
       return 1;
    }
    else
    {
       return -1;
    }
}

int EcatMasterBase::initialServoParameter(RobotBase *rob)
{
    return rob->initialServoParameter();
}


void* EcatMasterBase::threadTask( void* classPtr ){
    return ((EcatMasterBase*)classPtr)->run(NULL);
}

//pthread_create(&m_pid, NULL, &NDNMib::startProcessEventsHelper, this);

void EcatMasterBase::start()
{
    isShutDown=false;
//    cycle_us=us;
//    cycle_ns=cycle_us*1000;
    setMasterState(eMasterOP);

    int ret=pthread_create( &thId, NULL, &EcatMasterBase::threadTask, this);
    if(ret!=0)
    {
        printf("Create pthread printf!\n");
    }
    printf("\n\n--master thread start!--\n\n");
   // qDebug()<<"EcatMasterBase this=="<<this;
}

int EcatMasterBase::stop()
{
    isShutDown=true;
    isRun=false;
    if(masterCurrentState>eMasterPreOP)
    {
        setMasterState(eMasterPreOP);
    }
    else
    {
        isRun=false;
    }
    pthread_join(thId,NULL);
    return 1;
}

E_ECAT_MASTER_STATE EcatMasterBase::getMasterState()
{
    return (E_ECAT_MASTER_STATE)masterCurrentState;
}

void EcatMasterBase::setMasterState(E_ECAT_MASTER_STATE cmd)
{
    masterCommandState=cmd;
}

int EcatMasterBase::initialTimerMeasurement()
{
    #if D_IF_DEBUG
        int pttime=15000000/cycle_us;//15s 次数打印
        timer_id_measure[D_TIME_TIMER]=TimeTool::getInstance()->creatTimer( "timer",   cycle_ns, pttime,min_v_measure, max_v_measure , false);//
        timer_id_measure[D_TIME_RECEIVE]=TimeTool::getInstance()->creatTimer( "receive",       cycle_ns, pttime,min_v_measure, max_v_measure , false);//
        timer_id_measure[D_TIME_CUSTOM]=TimeTool::getInstance()->creatTimer( "custom_step1_updateDomainData",   cycle_ns, pttime,min_v_measure, max_v_measure , false);//
        timer_id_measure[D_TIME_MATH]=TimeTool::getInstance()->creatTimer( "custom_step1_setServoData", cycle_ns, pttime,min_v_measure, max_v_measure, false );//
        timer_id_measure[D_TIME_SEND]=TimeTool::getInstance()->creatTimer( "send",   cycle_ns, pttime,min_v_measure, max_v_measure, false );//
    #endif

    //#if D_IF_DEBUG
    //int pttime=15000000/cycle_us;//15s 次数打印
    //QString tmpStringFlag;
    //QString tmpString;
    //tmpStringFlag= "canbus";
    //tmpString="timer";
    //tmpString=tmpString+tmpStringFlag;
    //timer_id[D_TIME_TIMER]=TimeTool::getInstance()->creatTimer( tmpString, cycle_ns, pttime,min_v_CanbusCom, max_v_CanbusCom , false);//
    //tmpString="receive";
    //tmpString=tmpString+tmpStringFlag;
    //timer_id[D_TIME_RECEIVE]=TimeTool::getInstance()->creatTimer( tmpString, cycle_ns, pttime,min_v_CanbusCom, max_v_CanbusCom , false);//
    //tmpString="custom";
    //tmpString=tmpString+tmpStringFlag;
    //timer_id[D_TIME_CUSTOM]=TimeTool::getInstance()->creatTimer( tmpString, cycle_ns, pttime,min_v_CanbusCom, max_v_CanbusCom , false);//
    //tmpString="algorithm";
    //tmpString=tmpString+tmpStringFlag;
    //timer_id[D_TIME_MATH]=TimeTool::getInstance()->creatTimer( tmpString, cycle_ns, pttime,min_v_CanbusCom, max_v_CanbusCom, false );//
    //tmpString="send";
    //tmpString=tmpString+tmpStringFlag;
    //timer_id[D_TIME_SEND]=TimeTool::getInstance()->creatTimer( tmpString,   cycle_ns, pttime,min_v_CanbusCom, max_v_CanbusCom, false );//
    //#endif

    return 1;
}

//void EcatMasterBase::getProcessData(char* dataPtr, unsigned int pos, unsigned int bitLen);
//void EcatMasterBase::setProcessData(char* dataPtr, unsigned int pos, unsigned int bitLen);

#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void* EcatMasterBase::run(void *)
{
#if 1
    int max_cpus = sysconf(_SC_NPROCESSORS_ONLN);
    printf("Max CPUs = %d\n", max_cpus);

    /* lock all memory (prevent swapping) */
    if (mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        printf("printf: mlockall\n");
    }

    set_latency_target();
    #ifdef D_USE_IGH
    set_cpu(3);//igh 需要设置cpu亲和性
    #endif

#endif

    struct sched_param param;
//    param.__sched_priority = 99;
//    sched_setscheduler(0,SCHED_FIFO,&param);
    param.__sched_priority = pthread_priority;
    qDebug()<<"EcatMasterBase pid="<<gettid()<<"pthread_priority"<<pthread_priority;
    sched_setscheduler(0,pthread_schedule,&param);

    //setStackSize(81920);
    int policy, priority;
    pthread_getschedparam(pthread_self(), &policy, (struct sched_param *)&priority);
    if(mlockall(MCL_CURRENT|MCL_FUTURE) == -1) {
        qDebug("mlockall failed");
    }
//    clock_gettime( CLOCK_MONOTONIC, &cycleTimespec );
//    waitNextCycle();
    isrunning=true;
   // qDebug()<<"this=="<<this;

    while(isRun)
    {
        if(eMasterOP!=masterCurrentState && eMasterPreOP!=masterCurrentState )
        {
            usleep(10000);//10ms
            stateMachine();
        }
        else if(eMasterPreOP==masterCurrentState)
        {
            stateMachine();//preop to op ,no wait ,because of dc sync
//            wakeup_time = system_time_ns() + 10 * cycle_ns;
            wakeup_time = system_time_ns();
        }
        else if(eMasterOP==masterCurrentState)
        {
            stateMachine();
            cycle_counter++;
            wait_period();//DC-SLAVE USE
//            waitNextCycle();//dc-master use
        }
    }
    canquit=true;
    printf("Ethercat master had stopped!");
}

/****************************************************************************/

/** Get the time in ns for the current cpu, adjusted by system_time_base.
 *
 * \attention Rather than calling rt_get_time_ns() directly, all application
 * time calls should use this method instead.
 *
 * \ret The time in ns.
 */
uint64_t EcatMasterBase::system_time_ns(void)
{
    struct timespec tmpTime;
    clock_gettime( CLOCK_MONOTONIC, &tmpTime );

    uint64_t time = timespec2ns(tmpTime);

    if (system_time_base > time && system_time_base>0) {
        printf("%s() error: system_time_base greater than"
                " system time (system_time_base: %lld, time: %llu\n",
                __func__, system_time_base, time);
        return time;
    }
    else {
        return time - system_time_base;
    }
}

uint64_t EcatMasterBase::system_time_ns_no_plus_base()
{
    struct timespec tmpTime;
    clock_gettime( CLOCK_MONOTONIC, &tmpTime );

    uint64_t time = timespec2ns(tmpTime);
    return time;
}

uint64_t EcatMasterBase::timespec2ns(struct timespec timeSp)
{
    uint64_t tmpValue=timeSp.tv_sec;
    tmpValue=tmpValue * NSEC_PER_SEC;
    tmpValue+=timeSp.tv_nsec;
    return  tmpValue;
}

timespec EcatMasterBase::ns2timespec(uint64_t timeIn)
{
    timespec timeSp;
    timeSp.tv_sec=timeIn/NSEC_PER_SEC;
    timeSp.tv_nsec=timeIn%NSEC_PER_SEC;
    return timeSp;
}


/** Wait for the next period
 */
void EcatMasterBase::wait_period(void)
{
    // calc next wake time (in sys time)
    wakeup_time += cycle_ns;
    while (1)
    {
        uint64_t wakeup_count = system2count(wakeup_time);
        uint64_t current_count = system_time_ns_no_plus_base();

        if ((wakeup_count < current_count)
                || (wakeup_count > current_count + (50 * cycle_ns))) {
//            printf("%s(): unexpected wake time!\n", __func__);
        }
        struct timespec tmpTimeSleep=ns2timespec(wakeup_count);
        clock_nanosleep(CLOCK_MONOTONIC ,TIMER_ABSTIME, &tmpTimeSleep, NULL);

//        switch (rt_sleep_until(wakeup_count)) {
//            case RTE_UNBLKD:
//                rt_printk("rt_sleep_until(): RTE_UNBLKD\n");
//                continue;

//            case RTE_TMROVRN:
//                rt_printk("rt_sleep_until(): RTE_TMROVRN\n");
//                overruns++;

//                if (overruns % 100 == 0) {
//                    // in case wake time is broken ensure other processes get
//                    // some time slice (and error messages can get displayed)
//                    rt_sleep(cycle_ns / 100);
//                }
//                break;

//            default:
//                break;
//        }

        // done if we got to here
        break;
    }


}

/****************************************************************************/

/** Convert system time to RTAI time in counts (via the system_time_base).no nano2count
 */
uint64_t EcatMasterBase::system2count(uint64_t time)
{
    uint64_t ret;

    if ((system_time_base < 0) &&
            ((uint64_t) (-system_time_base) > time)) {
        printf("%s() error: system_time_base less than"
                " system time (system_time_base: %lld, time: %llu\n",
                __func__, system_time_base, time);
        ret = time;
    }
    else {
        ret = time + system_time_base;
    }

    return ret;
}

void EcatMasterBase::osSleepUs(int us)
{
    int sleep_ns = us*1000;
    struct timespec sleep_time;
    clock_gettime(CLOCK_MONOTONIC , &sleep_time);//
    sleep_time.tv_nsec += sleep_ns ;
    while (sleep_time.tv_nsec >= NSEC_PER_SEC)
    {
        sleep_time.tv_nsec -= NSEC_PER_SEC;
        sleep_time.tv_sec++;
    }
    clock_nanosleep(CLOCK_MONOTONIC , TIMER_ABSTIME, &sleep_time, NULL);
}


struct timespec EcatMasterBase::timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

void EcatMasterBase::waitNextCycle(){
//    struct timespec wakeupTime,cycle_ns_spec;
//    cycle_ns_spec.tv_sec=0;
//    cycle_ns_spec.tv_nsec=cycle_ns;

//    wakeupTime = timespec_add(wakeupTime, cycle_ns_spec);
//    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeupTime, NULL);
    //CLOCK_REALTIME,CLOCK_PROCESS_CPU_ID,CLOCK_THREAD_CPU_ID

//    struct timespec  now;
//    clock_gettime( CLOCK_MONOTONIC, &now );
//    //防止超过周期时间带来的连续超时
//    while(tsgreater(&now, &cycleTimespec))
//    {


//    wakeup_time = timespec_add(wakeup_time, cycle_ns);
//    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &wakeup_time, NULL);

}

//状态提升 (不可越级)
void EcatMasterBase::upgradeState()
{
    switch(masterCurrentState)
    {
    case eMasterErr_LOCK:
    {
        break;
    }

    case eMasterErr:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterUnknown;
        break;
    case eMasterBootstrap:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterUnknown;
        break;
    case eMasterUnknown:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterInit;
        break;
    case eMasterInit:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterPreOP;
        break;
    case eMasterPreOP:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterSafeOP;
        break;
    case eMasterSafeOP:
        ///TODO .. set slaves to next state
        masterCurrentState=eMasterOP;
        break;
    case eMasterOP:
        masterCommandState=eMasterOP;
        masterCurrentState=eMasterOP;
        break;
    default:
        break;
    }
   // qDebug()<<"nowState="<<nowState;
}

//状态回退：可越级
void EcatMasterBase::fallbackState(){

    int olds = masterCurrentState;
    switch(masterCurrentState){
    case eMasterErr:
        masterCommandState=eMasterErr;
        masterCurrentState=eMasterErr;
        break;
    case eMasterBootstrap:
        /// TODO .. exit state
        break;
    case eMasterUnknown:
        /// TODO .. exit state
        break;
    case eMasterInit:
        /// TODO .. exit state
        break;
    case eMasterPreOP:
        /// TODO .. exit state
        break;
    case eMasterSafeOP:
        /// TODO .. exit state
        break;
    case eMasterOP:
        /// TODO .. exit state
        break;
    default:
        break;
    }
    masterCurrentState=masterCommandState;
    if( olds!=masterCurrentState )
        masteroldState=olds;

    /// TODO .. set slaves to state: masterCtl
}
int EcatMasterBase::setDcCycle(int cycleTime)
{
    cycle_us=cycleTime;
    cycle_ns=cycle_us*1000;
    return 1;
}

void EcatMasterBase::stateMachine( )
{
    switch(masterCurrentState)
    {
    case eMasterErr_LOCK:
    {
        masterError();
//        printf(" error: eMasterErr_LOCK");
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9065, -1);
        break;
    }
    case eMasterOP:
        masterOp();
        break;
    case eMasterErr:
        masterError();
        break;
    case eMasterInit:
        masterInit();
        break;
    case eMasterPreOP:
        masterPreOp();
        break;
    case eMasterSafeOP:
        masterSafeOp();
        break;
    case eMasterBootstrap:
        masterBootStrap();
        break;
    case eMasterSleep:
        break;
    case eMasterUnknown:
        break;
    default:
        break;
    }

    if( masterCurrentState!=masterCommandState )
    {
        // exit old state and set to new state
        if(masterCommandState>masterCurrentState)
        {
            if(true==robCtl->busManager->getXmlFilesStatus())
            {
                upgradeState();
            }
        }
        else
        {
            fallbackState();
        }
       // printf( "stateMachine == %d, %d \n", masterCtl, nowState );
    }

}


//#include <AtEthercat.h>

//#include "DCDemo.h"
//#include "Logging.h"
//#include "motrotech.h"

//#ifdef ATEMRAS_SERVER
//#include <AtEmRasSrv.h>
//#endif

//void mystep(){
//    ///myAppInit

//    int dwRes;

//    ///  ATEMRAS_SERVER
//    int dwRes = emRasSrvStart(oRemoteApiConfig, &S_pvRemoteApiSrvH);
//    if (EC_E_NOprintf != dwRes)
//    {
//        Logprintf("printf: Cannot spawn Remote API Server\n");
//    }

//    ///  /* Initialize EtherCAT master */
//    EC_T_INIT_MASTER_PARMS oInitParms;

//    OsMemset(&oInitParms, 0, sizeof(EC_T_INIT_MASTER_PARMS));
//    oInitParms.dwSignature                   = ATECAT_SIGNATURE;
//    oInitParms.dwSize                        = sizeof(EC_T_INIT_MASTER_PARMS);
//    oInitParms.pLinkParms                    = poLinkParms;
//    oInitParms.pLinkParmsRed                 = poLinkParmsRed;
//    oInitParms.dwBusCycleTimeUsec            = dwBusCycleTimeUsec;
//    oInitParms.dwMaxBusSlaves                = MASTER_CFG_ECAT_MAX_BUS_SLAVES;
//    oInitParms.dwMaxQueuedEthFrames          = MASTER_CFG_MAX_QUEUED_ETH_FRAMES;
//    oInitParms.dwMaxSlaveCmdPerFrame         = MASTER_CFG_MAX_SLAVECMD_PER_FRAME;
//    if (dwBusCycleTimeUsec < 1000)
//    {
//        oInitParms.dwMaxSentQueuedFramesPerCycle = 1;
//    }
//    else
//    {
//        oInitParms.dwMaxSentQueuedFramesPerCycle = MASTER_CFG_MAX_SENT_QUFRM_PER_CYC;
//    }
//    oInitParms.dwEcatCmdMaxRetries           = MASTER_CFG_ECAT_CMD_MAX_RETRIES;
//    oInitParms.dwEoETimeout                  = MASTER_CFG_EOE_TIMEOUT;
//    oInitParms.dwFoEBusyTimeout              = MASTER_CFG_FOE_BUSY_TIMEOUT;
//    oInitParms.dwLogLevel                    = nVerbose;
//    oInitParms.pfLogMsgCallBack              = CAtEmLogging::OsDbgMsgHookWrapper;
//    oInitParms.pOsParms                      = poOsParms;

//    dwRes = ecatInitMaster(&oInitParms);
//    dwRes = ecatInitMaster(&oInitParms);

//    /// task thread start  // run();


//    ///  /* Configure master */
//    dwRes = ecatConfigureMaster(eCnfType, pbyCnfData, dwCnfDataLen);
//    if (dwRes != EC_E_NOprintf)
//    {
//        dwRetVal = dwRes;
//        Logprintf("Cannot configure EtherCAT-Master! %s (Result = 0x%x)", ecatGetText(dwRes), dwRes);
//        goto Exit;
//    }


//    /* configure DC/DCM if master is started with ENI */
//    if (pbyCnfData != EC_NULL)
//    {
//        /* configure DC */
//        {
//            EC_T_DC_CONFIGURE oDcConfigure;

//            OsMemset(&oDcConfigure, 0, sizeof(EC_T_DC_CONFIGURE));
//            oDcConfigure.dwTimeout          = ETHERCAT_DC_TIMEOUT;
//            oDcConfigure.dwDevLimit         = ETHERCAT_DC_DEV_LIMIT;
//            oDcConfigure.dwSettleTime       = ETHERCAT_DC_SETTLE_TIME;
//            if (eDcmMode_MasterRefClock == eDcmMode)
//            {
//                oDcConfigure.dwTotalBurstLength = 10000;
//                oDcConfigure.dwBurstBulk        = 1;
//            }
//            else
//            {
//                oDcConfigure.dwTotalBurstLength = ETHERCAT_DC_ARMW_BURSTCYCLES;
//                if (dwBusCycleTimeUsec < 1000)
//                {
//                    /* if the cycle time is below 1000 usec, we have to reduce the number of frames sent within one cycle */
//                    oDcConfigure.dwBurstBulk = ETHERCAT_DC_ARMW_BURSTSPP / 2;
//                }
//                else
//                {
//                    oDcConfigure.dwBurstBulk = ETHERCAT_DC_ARMW_BURSTSPP;
//                }
//            }
//            if (eDcmMode_Dcx == eDcmMode)
//            {
//                oDcConfigure.bAcycDistributionDisabled = EC_FALSE; /* Enable acyclic distribution if cycle time is above 1000 usec to get DCX in sync */
//            }
//            else
//            {
//                oDcConfigure.bAcycDistributionDisabled = EC_TRUE;
//            }
//            dwRes = ecatDcConfigure(&oDcConfigure);
//            if (dwRes != EC_E_NOprintf )
//            {
//                dwRetVal = dwRes;
//                Logprintf("Cannot configure DC! (Result = 0x%x)", dwRes);
//                goto Exit;
//            }
//        }
//        /* configure DCM */
//        {
//            EC_T_DCM_CONFIG oDcmConfig;
//            EC_T_BOOL       bLogEnabled = EC_FALSE;
//#ifdef DCM_ENABLE_LOGFILE
//            if (pNotification->Verbose() >= 3)
//            {
//                bLogEnabled = EC_TRUE;
//            }
//#endif
//            if (bDcmCtlOff)
//            {
//                if (0 != nVerbose) LogMsg("DCM control loop disabled for diagnosis!\n");
//            }
//            OsMemset(&oDcmConfig, 0, sizeof(EC_T_DCM_CONFIG));
//            switch (eDcmMode)
//            {
//            case eDcmMode_Off:
//                oDcmConfig.eMode = eDcmMode_Off;
//                break;
//            case eDcmMode_BusShift:
//                oDcmConfig.eMode = eDcmMode_BusShift;
//                oDcmConfig.u.BusShift.nCtlSetVal    = nDcmCtlSetVal;
//                oDcmConfig.u.BusShift.dwInSyncLimit = (dwBusCycleTimeUsec*1000)/5;    /* 20 % limit in nsec for InSync monitoring */
//                oDcmConfig.u.BusShift.bLogEnabled = bLogEnabled;
//                oDcmConfig.u.BusShift.bCtlOff = bDcmCtlOff;
//                break;
//            case eDcmMode_MasterShift:
//                oDcmConfig.eMode = eDcmMode_MasterShift;
//                oDcmConfig.u.MasterShift.nCtlSetVal    = nDcmCtlSetVal;
//                oDcmConfig.u.MasterShift.dwInSyncLimit = (dwBusCycleTimeUsec*1000)/5;    /* 20 % limit in nsec for InSync monitoring */
//                oDcmConfig.u.MasterShift.bLogEnabled = bLogEnabled;
//                oDcmConfig.u.MasterShift.bCtlOff = bDcmCtlOff;
//                break;
//            case eDcmMode_MasterRefClock:
//                oDcmConfig.eMode = eDcmMode_MasterRefClock;
//                oDcmConfig.u.MasterRefClock.nCtlSetVal  = nDcmCtlSetVal;
//                oDcmConfig.u.MasterRefClock.bLogEnabled = bLogEnabled;
//                break;
//            case eDcmMode_Dcx:
//                oDcmConfig.eMode = eDcmMode_Dcx;
//                /* Mastershift */
//                oDcmConfig.u.Dcx.MasterShift.nCtlSetVal = nDcmCtlSetVal;
//                oDcmConfig.u.Dcx.MasterShift.dwInSyncLimit = (dwBusCycleTimeUsec * 1000) / 5;    /* 20 % limit in nsec for InSync monitoring */
//                oDcmConfig.u.Dcx.MasterShift.bLogEnabled = bLogEnabled;
//                oDcmConfig.u.Dcx.MasterShift.bCtlOff = bDcmCtlOff;
//                /* Dcx Busshift */
//                oDcmConfig.u.Dcx.nCtlSetVal = nDcmCtlSetVal;
//                oDcmConfig.u.Dcx.dwInSyncLimit = (dwBusCycleTimeUsec * 1000) / 5;    /* 20 % limit in nsec for InSync monitoring */
//                oDcmConfig.u.Dcx.bLogEnabled = bLogEnabled;
//                oDcmConfig.u.Dcx.bCtlOff = bDcmCtlOff;
//                oDcmConfig.u.Dcx.dwExtClockTimeout = 1000;
//                oDcmConfig.u.Dcx.wExtClockFixedAddr = 0; /* 0 only when clock adjustment in external mode configured by EcEngineer */
//                break;
//            default:
//                dwRetVal = EC_E_NOTSUPPORTED;
//                Logprintf("DCM mode is not supported!");
//                goto Exit;

//            }
//            dwRes = ecatDcmConfigure(&oDcmConfig, 0);
//            switch (dwRes)
//            {
//            case EC_E_NOprintf:
//                break;
//            case EC_E_FEATURE_DISABLED:
//                Logprintf("Cannot configure DCM mode!");
//                Logprintf("Start with -dcmmode off to run the DC demo without DCM, or prepare the ENI file to support the requested DCM mode");
//                Logprintf("In ET9000 for example, select under ""Advanced settings\\Distributed clocks"" ""DC in use"" and ""Slave Mode""");
//                Logprintf("to support BusShift and MasterRefClock modes.");
//                Logprintf("Please refer to the class A manual for further information");
//                dwRetVal = dwRes;
//                goto Exit;
//            default:
//                Logprintf("Cannot configure DCM mode! %s (Result = 0x%x)", ecatGetText(dwRes), dwRes);
//                dwRetVal = dwRes;
//                goto Exit;
//            }
//        }
//    }

//    ///

//    /* Register client */
//    {
//        EC_T_REGISTERRESULTS oRegisterResults;

//        OsMemset(&oRegisterResults, 0, sizeof(EC_T_REGISTERRESULTS));
//        dwRes = ecatRegisterClient(ecatNotifyCallback, pNotification, &oRegisterResults);
//        if (dwRes != EC_E_NOprintf)
//        {
//            dwRetVal = dwRes;
//            Logprintf("Cannot register client! (Result = 0x%x)", dwRes);
//            goto Exit;
//        }
//        S_dwClntId = oRegisterResults.dwClntId;
//        pNotification->SetClientID(S_dwClntId);
//    }

//    /* Print found slaves */
//    if (nVerbose >= 2)
//    {
//        dwRes = ecatScanBus(ETHERCAT_SCANBUS_TIMEOUT);
//        switch (dwRes)
//        {
//        case EC_E_NOprintf:
//        case EC_E_BUSCONFIG_MISMATCH:
//        case EC_E_LINE_CROSSED:
//            PrintSlaveInfos(INSTANCE_MASTER_DEFAULT, poLog);
//            break;
//        default:
//            Logprintf("Cannot scan bus: %s (0x%lx)", ecatGetText(dwRes), dwRes);
//            break;
//        }
//    }

//    /* Print MAC address */
//    if (nVerbose > 0)
//    {
//        ETHERNET_ADDRESS oSrcMacAddress;

//        dwRes = ecatGetSrcMacAddress(&oSrcMacAddress);
//        if (dwRes != EC_E_NOprintf)
//        {
//            Logprintf("Cannot get MAC address! (Result = 0x%x)", dwRes);
//        }
//        LogMsg("EtherCAT network adapter MAC: %02X-%02X-%02X-%02X-%02X-%02X\n",
//               oSrcMacAddress.b[0], oSrcMacAddress.b[1], oSrcMacAddress.b[2], oSrcMacAddress.b[3], oSrcMacAddress.b[4], oSrcMacAddress.b[5]);
//    }


//    if (0 != nVerbose) LogMsg( "=====================" );
//    if (0 != nVerbose) LogMsg( "Start EtherCAT Master" );
//    if (0 != nVerbose) LogMsg( "=====================" );

//    /* set master and bus state to INIT */
//    dwRes = ecatSetMasterState(ETHERCAT_STATE_CHANGE_TIMEOUT, eEcatState_INIT);
//    pNotification->ProcessNotificationJobs();
//    if (dwRes != EC_E_NOprintf)
//    {
//        Logprintf("Cannot start set master state to INIT (Result = %s (0x%lx))", ecatGetText(dwRes), dwRes);
//        dwRetVal = dwRes;
//        goto Exit;
//    }



//    ///bus检测
//    for (EC_T_DWORD i = 0; i < emGetNumConnectedSlaves(INSTANCE_MASTER_DEFAULT); i++)
//    {
//        if (FindSlaveGetFixedAddr(INSTANCE_MASTER_DEFAULT, poLog, i, dwVendorId, dwProductCode, &wFixedAddress))
//        {
//            if ((FoundFixedAddress != wFixedAddress) && (wFixedAddress >= 1001))
//            {
//                (*slave_num)++;
//                MotorCount++;
//                FoundFixedAddress = wFixedAddress;

//                LogMsg("Motrotech: (%d)my drive have find",MotorCount);
//                //		        }
//            }
//        }
//    }

//    ///用厂商 ，版本号 来匹配伺服 ， （其他的就是 io/...）
//    if (FindSlaveGetFixedAddr(INSTANCE_MASTER_DEFAULT, poLog, 0, ecvendor_beckhoff, ecprodcode_beck_EL1014, &wFixedAddress))
//    {
//        S_dwSlaveIdx14 = S_dwAppFoundSlaves;
//    }
//    if (S_dwSlaveIdx14 != SLAVE_NOT_FOUND)
//    {
//        S_dwAppFoundSlaves++;

//        /* now get the offset of this device in the process data buffer and some other infos */
//        if (ecatGetCfgSlaveInfo(EC_TRUE, wFixedAddress, &S_aSlaveList[S_dwSlaveIdx14]) != EC_E_NOprintf)
//        {
//            Logprintf("printf: ecatGetCfgSlaveInfo() returns with printf.");
//        }
//    }


//    ///然后 切换 状态
//    /* set master and bus state to PREOP */
//    dwRes = ecatSetMasterState(ETHERCAT_STATE_CHANGE_TIMEOUT, eEcatState_PREOP);



//    /* set master and bus state to PREOP */
//    dwRes = ecatSetMasterState(ETHERCAT_STATE_CHANGE_TIMEOUT, eEcatState_PREOP);
//    pNotification->ProcessNotificationJobs();
//    if (dwRes != EC_E_NOprintf)
//    {
//        Logprintf("Cannot start set master state to PREOP (Result = %s (0x%lx))", ecatGetText(dwRes), dwRes);
//        dwRetVal = dwRes;
//        goto Exit;
//    }
//    /* skip this step if demo started without ENI */
//    if (pbyCnfData != EC_NULL)
//    {
//        /******************************************************/
//        /* Demo code: Remove/change this in your application  */
//        /******************************************************/
//        dwRes = myAppSetup(poLog, nVerbose, S_dwClntId);
//        if (EC_E_NOprintf != dwRes)
//        {
//            Logprintf((EC_T_CHAR*)"myAppSetup failed, printf code: 0x%x", dwRes);
//            dwRetVal = dwRes;
//            goto Exit;
//        }
//        /* set master and bus state to SAFEOP */
//        dwRes = ecatSetMasterState(ETHERCAT_STATE_CHANGE_TIMEOUT, eEcatState_SAFEOP);
//        pNotification->ProcessNotificationJobs();
//        if (dwRes != EC_E_NOprintf)
//        {
//            Logprintf("Cannot start set master state to SAFEOP (Result = %s (0x%lx))", ecatGetText(dwRes), dwRes);

//            /* most of the time SAFEOP is not reachable due to DCM */
//            if (eDcmMode_Off != eDcmMode)
//            {
//                EC_T_DWORD dwDcmStatus = 0;
//                EC_T_INT   nDiffCur = 0, nDiffAvg = 0, nDiffMax = 0;

//                dwRes = ecatDcmGetStatus(&dwDcmStatus, &nDiffCur, &nDiffAvg, &nDiffMax);
//                if (dwRes == EC_E_NOprintf)
//                {
//                    if (dwDcmStatus != EC_E_NOprintf)
//                    {
//                        Logprintf("DCM Status: %s (0x%08X)\n", ecatGetText(dwDcmStatus), dwDcmStatus);
//                    }
//                }
//                else
//                {
//                    Logprintf("Cannot get DCM status! %s (0x%08X)\n", ecatGetText(dwRes), dwRes);
//                }
//            }
//            dwRetVal = dwRes;
//            goto Exit;
//        }
//        /* set master and bus state to OP */
//        dwRes = ecatSetMasterState(ETHERCAT_STATE_CHANGE_TIMEOUT, eEcatState_OP);
//        pNotification->ProcessNotificationJobs();
//        if (dwRes != EC_E_NOprintf)
//        {
//            Logprintf("Cannot start set master state to OP (Result = %s (0x%lx))", ecatGetText(dwRes), dwRes);
//            dwRetVal = dwRes;
//            goto Exit;
//        }
//    }
//    else
//    {
//        if (0 != nVerbose) LogMsg("No ENI file provided. EC-Master started with generated ENI file.");
//    }

//    if (S_bEnaPerfJobs)
//    {
//        LogMsg("");
//        LogMsg("Job times during startup <INIT> to <%s>:", ecatStateToStr(ecatGetMasterState()));
//        PERF_MEASURE_JOBS_SHOW();       /* show job times */
//        LogMsg("");
//        ecatPerfMeasReset(&S_TscMeasDesc, 0xFFFFFFFF);        /* clear job times of startup phase */
//    }

//#if (defined DEBUG) && (defined XENOMAI)
//    /* Enabling mode switch warnings for shadowed task */
//    dwRes = rt_task_set_mode(0, T_WARNSW, NULL);
//    if (0 != dwRes)
//    {
//        OsDbgMsg("EnableRealtimeEnvironment: rt_task_set_mode returned an printf\n");
//        OsDbgAssert(EC_FALSE);
//    }
//#endif

//    /* run the demo */
//    if (dwDuration != 0)
//    {
//        oTimeout.Start(dwDuration);
//    }

//    for(EC_T_DWORD i = 0; i < 6; i++)
//    {
//        Process_Switch(i, COMMAND_START);
//    }

//    while (bRun && (!oTimeout.IsStarted() || !oTimeout.IsElapsed()))
//    {
//        if (nVerbose >= 2)
//        {
//            PERF_MEASURE_JOBS_SHOW();       /* show job times */
//        }
//        bRun = !OsTerminateAppRequest();/* check if demo shall terminate */

//        /*****************************************************************************************/
//        /* Demo code: Remove/change this in your application: Do some diagnosis outside job task */
//        /*****************************************************************************************/
//        myAppDiagnosis(poLog, nVerbose);

//        /* check DCM status if master is started with ENI */
//        if (pbyCnfData != EC_NULL)
//        {
//            if (eDcmMode_Off != eDcmMode)
//            {
//                EC_T_DWORD dwDcmStatus = 0;
//                EC_T_INT   nDiffCur = 0, nDiffAvg = 0, nDiffMax = 0;
//                EC_T_BOOL  bLogDcmStatus = EC_FALSE;
//                EC_T_BOOL  bResetDcmStatus = bFirstDcmStatus;

//                if (!oDcmStatusTimer.IsStarted() || oDcmStatusTimer.IsElapsed())
//                {
//                    oDcmStatusTimer.Start(5000);
//                    if (nVerbose >= 3)
//                    {
//                        bLogDcmStatus = EC_TRUE;
//                    }
//                }
//                dwRes = ecatDcmGetStatus(&dwDcmStatus, &nDiffCur, &nDiffAvg, &nDiffMax);
//                if (dwRes == EC_E_NOprintf)
//                {
//                    if (bFirstDcmStatus)
//                    {
//                        LogMsg("DCM during startup (<INIT> to <%s>)", ecatStateToStr(ecatGetMasterState()));
//                    }
//                    if ((dwDcmStatus != EC_E_NOTREADY) && (dwDcmStatus != EC_E_BUSY) && (dwDcmStatus != EC_E_NOprintf))
//                    {
//                        LogMsg("DCM Status: %s (0x%08X)\n", ecatGetText(dwDcmStatus), dwDcmStatus);
//                    }
//                    if (bLogDcmStatus)
//                    {
//                        LogMsg("DCM Diff(ns): Cur=%7d, Avg=%7d, Max=%7d", nDiffCur, nDiffAvg, nDiffMax);
//                    }
//                }
//                else
//                {
//                    if ((eEcatState_OP == ecatGetMasterState()) || (eEcatState_SAFEOP == ecatGetMasterState()))
//                    {
//                        Logprintf("Cannot get DCM status! %s (0x%08X)\n", ecatGetText(dwRes), dwRes);
//                    }
//                    bResetDcmStatus = EC_FALSE;
//                }
//                if (eDcmMode_Dcx == eDcmMode)
//                {
//                    EC_T_INT64 qwTimeStampDiff = 0;

//                    dwRes = ecatDcxGetStatus(&dwDcmStatus, &nDiffCur, &nDiffAvg, &nDiffMax, &qwTimeStampDiff);
//                    if (EC_E_NOprintf == dwRes)
//                    {
//                        if (bFirstDcmStatus)
//                        {
//                            LogMsg("DCX during startup (<INIT> to <%s>)", ecatStateToStr(ecatGetMasterState()));
//                        }
//                        if ((dwDcmStatus != EC_E_NOTREADY) && (dwDcmStatus != EC_E_BUSY) && (dwDcmStatus != EC_E_NOprintf))
//                        {
//                            LogMsg("DCX Status: %s (0x%08X)\n", ecatGetText(dwDcmStatus), dwDcmStatus);
//                        }
//                        if (bLogDcmStatus)
//                        {
//                            LogMsg("DCX Diff(ns): Cur=%7d, Avg=%7d, Max=%7d, TimeStamp=%7d", nDiffCur, nDiffAvg, nDiffMax, qwTimeStampDiff);
//                        }
//                    }
//                    else
//                    {
//                        if ((eEcatState_OP == ecatGetMasterState()) || (eEcatState_SAFEOP == ecatGetMasterState()))
//                        {
//                            Logprintf("Cannot get DCX status! %s (0x%08X)\n", ecatGetText(dwRes), dwRes);
//                        }
//                        bResetDcmStatus = EC_FALSE;
//                    }
//                }
//                if (bResetDcmStatus)
//                {
//#if (defined ATECAT_VERSION) && (ATECAT_VERSION >= 0x02040106)
//                    ecatDcmResetStatus();
//#endif
//                    bFirstDcmStatus = EC_FALSE;
//                }
//            }
//        }
//        /* process notification jobs */
//        pNotification->ProcessNotificationJobs();

//        OsSleep(5);
//    }

//    if (S_bEnaPerfJobs)
//    {
//        LogMsg("");
//        LogMsg("Job times before shutdown");
//        PERF_MEASURE_JOBS_SHOW();       /* show job times */
//    }

//    for(EC_T_DWORD i = 0; i < 6; i++)
//    {
//        Process_Switch(i, COMMAND_STOP);
//    }

//Exit:
//    //==========
//}

int EcatMasterBase::readCheckConfig(QString fileNameIn,int &wkcCheckCountOut
                                   ,int &periodOut,int &receiveOut,int & sendOut,int &customOut)
{
    qDebug()<<"readCheckConfig,fileName="<<fileNameIn;
    QSettings settings(fileNameIn, QSettings::IniFormat);

    QString strConfigKey;

    strConfigKey= "common/wkcLostLimit";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,checkConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9060, -1);
        return -1;
    }
    wkcCheckCountOut = settings.value(strConfigKey).toInt();

    strConfigKey= "common/ecatPeriodTimerOffsetMax";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,checkConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9061, -1);
        return -1;
    }
    periodOut = settings.value(strConfigKey).toInt();

    strConfigKey= "common/ecatReceiveMax";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,checkConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9062, -1);
        return -1;
    }
    receiveOut = settings.value(strConfigKey).toInt();

    strConfigKey= "common/ecatSendMax";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,checkConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9063, -1);
        return -1;
    }
    sendOut = settings.value(strConfigKey).toInt();


    strConfigKey= "common/ecatCustomMax";
    if(false==settings.contains(strConfigKey))
    {
        qDebug()<<"error,,,,,checkConfig.ini loose parameter";
        addMsg(ENUM_MSG_ERROR, "EcatMasterBase", "EcatMasterBase" , 9064, -1);
        return -1;
    }
    customOut = settings.value(strConfigKey).toInt();


    qDebug()<<"----------------------readCheckConfig,wkcCheckCount="<<wkcCheckCountOut;
    return 1;
}

void EcatMasterBase::addMsg(int messageLevel, std::string componentName, std::string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
//    tmpMsg.time = t1.tv_sec;

    QString infomationStr;

    switch(messageCode)
    {
    case 9065:
    {
        infomationStr =  QObject::tr("主站处于错误锁死状态，请重启控制器 !");
        break;

    }
    case 9064:
    {
        infomationStr =  QObject::tr("ethercat checkConfig.ini文件缺少 ecatCustommax参数 !");
        break;

    }
    case 9063:
    {
        infomationStr =  QObject::tr("ethercat checkConfig.ini文件缺少 ecatSendMax参数 !");
        break;

    }
    case 9062:
    {
        infomationStr =  QObject::tr("ethercat checkConfig.ini文件缺少 ecatReceiveMax参数 !");
        break;

    }

    case 9061:
    {
        infomationStr =  QObject::tr("ethercat checkConfig.ini文件缺少 ecatPeriodTimerOffsetMax参数 !");
        break;

    }
    case 9060:
    {
        infomationStr =  QObject::tr("ethercat checkConfig.ini文件缺少 wkcLostLimit参数 !");
        break;

    }

    default:
    {

    }
    }

    tmpMsg.MessageInformation = infomationStr.toStdString();
    MessageLog::getInstance()->addMessage(tmpMsg);

}





